#pragma config(Sensor, in1,    ProgramPot,          sensorPotentiometer)
#pragma config(Sensor, dgtl1,  ArmEncoderLeft,      sensorRotation)
#pragma config(Sensor, dgtl2,  ArmEncoderRight,     sensorRotation)
#pragma config(Sensor, dgtl3,  BaseEncoderLeft,     sensorRotation)
#pragma config(Sensor, dgtl4,  BaseEncoderRight,    sensorRotation)
#pragma config(Sensor, dgtl5,  LimitArmOne,         sensorTouch)
#pragma config(Sensor, dgtl6,  LimitArmTwo,         sensorTouch)
#pragma config(Motor,  port2,           LeftBase,      tmotorNormal, openLoop)
#pragma config(Motor,  port10,           RightBase,     tmotorNormal, openLoop)
#pragma config(Motor,  port4,           LeftArm,       tmotorNormal, openLoop)
#pragma config(Motor,  port5,           RightArm,      tmotorNormal, openLoop)
#pragma config(Motor,  port6,           LeftVacume,    tmotorNormal, openLoop)
#pragma config(Motor,  port7,           RightVacume,   tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port8,           LeftTurntable, tmotorNormal, openLoop)
#pragma config(Motor,  port9,           RightTurntable, tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

///////////////////////////////Start Autonomous Control Voids/////////////////////////////
const int SquareAndAHalf = 36;
const int Square = 24;
bool VirtualButton = false;
void ResetEncoders()
{
  SensorValue(BaseEncoderLeft) = 0;
  SensorValue(BaseEncoderRight) = 0;
}

int DistanceToCounts(int distance)//Distance in 1/10 inches!
{
  int circumference;//This variable can only go to tenths decimal
  int counts = 0;//1 count = 4 degrees, initially set to zero

  circumference = (8.6 * 10);//Circumference of small Omni wheel. This can be changed anytime. In one-tenth inches units.
  counts = (distance * 90) / (circumference);//Set counts for proper distance
  return counts;
}

void DriveMotorStop()
{
  motor[LeftBase] = 0;
  motor[RightBase] = 0;
}

void VacumePickUp()
{
  motor[LeftVacume] = 127;
  motor[RightVacume] = 127;
  wait1Msec(1000);
  motor[LeftVacume] = 0;
  motor[RightVacume] = 0;
}

void VacumeSpitOut()
{
  while(1)
  {
    motor[LeftVacume] = 127;
    motor[RightVacume] = 127;
  }
}

void MoveStraight(int distance)
{

   int counts;//This is a variable to hold distance in counts

  ResetEncoders();//Reset encoders with function, start at zero!

  counts = DistanceToCounts(distance);

  while (SensorValue(BaseEncoderRight)<counts)
  {
    if (SensorValue(BaseEncoderLeft)>SensorValue(BaseEncoderRight))//Left faster
    {
      motor[LeftBase]=93;
      motor[RightBase]=103;
    }
    if (SensorValue(BaseEncoderLeft)<SensorValue(BaseEncoderRight))//Right faster
    {
      motor[LeftBase]=103;
      motor[RightBase]=93;
    }
    if (SensorValue(BaseEncoderLeft)==SensorValue(BaseEncoderRight))//Equal
    {
      motor[LeftBase]=103;
      motor[RightBase]=103;
    }
  }
  DriveMotorStop();
}


void MoveLift(int wantedposition)
{
  if(SensorValue(ArmEncoderLeft)<wantedposition && SensorValue(LimitArmOne) == 0)
  {
    while((SensorValue(ArmEncoderLeft)<wantedposition))
    {
      motor[LeftArm]=127;
      motor[RightArm]=127;
    }
    motor[LeftArm]=0;
    motor[RightArm]=0;
    return;
  }

  if(SensorValue(ArmEncoderLeft)>wantedposition)
  {
    while((SensorValue(ArmEncoderLeft)>wantedposition))
    {
      motor[LeftArm]=-127;
      motor[RightArm]=-127;
    }
    motor[LeftArm]=0;
    motor[RightArm]=0;
    return;
  }
  return;
}

void MoveLiftDown()
{
  VirtualButton = true;
  while(VirtualButton == true)
  {
    if(SensorValue[LimitArmOne] == 0)
    {
      motor[LeftArm] = -127;
    }
    if(SensorValue[LimitArmTwo] == 0)
    {
      motor[RightArm] = -127;
    }
    if(SensorValue[LimitArmOne] == 1)
    {
      motor[LeftArm] = 0;
    }
    if(SensorValue[LimitArmTwo] == 1)
    {
      motor[RightArm] = 0;
    }
    if(SensorValue[LimitArmOne] == 1 && SensorValue[LimitArmTwo] == 1)
    {
      VirtualButton = false;
    }
  }
}

void TurnRobot (int turnangle)
{
  int wheelseperation = 10;   //Width of robot base
  int rotationdistance;       //Distance wheel needs to turn in 1/10 inches
  int wheelcounts;            //Counts wheel needs to travel
  int absturnangle;           //Absalute value of turnangle
  bool negative = false;      //If a number is negative, this variable gets notated "true."

  absturnangle = 0;

  if(turnangle<0)//If turnangle variable is negative...
  {
    negative = true;//...Then notate it throught variables.
  }

  absturnangle = abs(turnangle);//Compute absolute value (-90 becomes 90 if negative or 90 stays 90)

  rotationdistance = (wheelseperation * absturnangle * 3.14159265) / 36;  //Compute distance in 1/10"; PI = 3.14159265 here
  wheelcounts = DistanceToCounts(rotationdistance);  //Turn distance into counts
  bMotorFlippedMode[port3] = true;  //No reflection

  ResetEncoders();  //Reset encoders for use.

  if(negative == false)
  {
    while(SensorValue(BaseEncoderLeft)<wheelcounts)  //Runs when needed.
    {
      motor[LeftBase] = -63;
      motor[RightBase] = 63;
    }
  }
  else
  {
    while(SensorValue(BaseEncoderLeft)<wheelcounts)
    {
      motor[LeftBase]= 63;
      motor[RightBase]= -63;
    }
  }
  DriveMotorStop();
}

//**Port Mapping**//
//All port 2's are left side
//All port 3's are right side

/////////////////////////////////End Autonomous Control Voids///////////////////////////////
task main()
{
    MoveStraight(SquareAndAHalf); // type in value for 1/10 of inch 240 = 1 square or 24 in. 1 square = 2 ft.
    TurnRobot(90);
    MoveStraight(Square);
    TurnRobot(-90);
    MoveStraight(SquareAndAHalf);
    VacumePickUp();
    VacumeSpitOut();
}
